import numpy as np
from pyproj import CRS, Transformer

# 创建 WGS84 和 ECEF 转换器
wgs84 = CRS.from_epsg(4326)  # WGS84地理坐标系
ecef = CRS.from_epsg(4978)   # ECEF地心坐标系
transformer_to_ecef = Transformer.from_crs("EPSG:4326", "EPSG:4978", always_xy=True)

# DMS 到十进制度转换函数
def dms_to_decimal(degrees, minutes, seconds, direction):
    try:
        decimal = degrees + minutes / 60 + seconds / 3600
        if direction in ['S', 'W']:
            decimal = -decimal  # 南纬和西经取负
        return decimal
    except Exception as e:
        print(f"转换DMS到十进制度时出错: {e}")
        return None

# 将 BLH（纬度、经度、高程）转换为 ECEF 坐标
def blh_to_ecef(blh):
    try:
        # DMS 转换为十进制度
        latitude = dms_to_decimal(*blh[:4])
        longitude = dms_to_decimal(*blh[4:8])
        height = blh[8]

        if latitude is None or longitude is None:
            raise ValueError("纬度或经度转换失败")

        # 使用 pyproj 进行 BLH 到 ECEF 的转换
        ecef = np.array(transformer_to_ecef.transform(longitude, latitude, height))
        return ecef
    except Exception as e:
        print(f"BLH到ECEF转换时出错: {e}")
        return None

# 计算平移向量
def compute_translation_vector(target_blh):
    target_ecef = blh_to_ecef(target_blh)
    
    if target_ecef is None:
        return None

    try:
        # 赤道原点的 ECEF 坐标直接计算
        origin_ecef = np.array(transformer_to_ecef.transform(0, 0, 0))
        
        # 计算平移向量
        translation_vector = target_ecef - origin_ecef
        print(f"目标位置 (ECEF): {target_ecef}")
        print(f"赤道原点 (ECEF): {origin_ecef}")

        # 处理一下向量，保证其在限度内
        if (translation_vector < -2**31).any() or (translation_vector > 2**31 - 1).any():
            raise ValueError("平移后的点云坐标超出了可接受的范围")
        
        # # 去除向量的小数部分
        # translation_vector = np.round(translation_vector).astype(int)
        # print(f"平移向量: {translation_vector}")

        return translation_vector
    except Exception as e:
        print(f"计算平移向量时出错: {e}")
        return None

# 计算平移向量
def compute_translation_vector2(target_blh):
    # 将目标位置的 BLH 坐标转换为十进制
    target_lat = dms_to_decimal(*target_blh[:4])
    target_lon = dms_to_decimal(*target_blh[4:8])
    target_height = target_blh[8]

    if target_lat is None or target_lon is None:
        return None

    # 赤道原点的经纬度和高程
    origin_lat = 0.0
    origin_lon = 0.0
    origin_height = 0.0

    # 计算平移向量
    translation_vector = np.array([target_lon - origin_lon, 
                                   target_lat - origin_lat, 
                                   target_height - origin_height])

    print(f"目标位置 (WGS84): ({target_lat}, {target_lon}, {target_height})")
    print(f"赤道原点 (WGS84): ({origin_lat}, {origin_lon}, {origin_height})")
    print(f"平移向量: {translation_vector}")

    return translation_vector


if __name__ == '__main__':
    target_blh = (40, 23, 30.19211, 'N', 115, 54, 43.52697, 'E', 496.885)
    # blh_example = [39, 54, 0, 'N', 116, 23, 0, 'E', 50]  # 北京市大约位置
    # ecef_coords = blh_to_ecef(blh_example)
    # print(f"ECEF坐标: {ecef_coords}")
    translation_vector = compute_translation_vector2(target_blh)
    print(f"平移向量: {translation_vector}")
